#!/usr/bin/python
import numpy as np
import pymoos
import time
import cv2

comms = pymoos.comms()

class AppCastingMOOSApp(object):
	def __init__(self):
		comms.set_on_connect_callback(self.RegisterVariables)
		comms.add_active_queue('OnNewMail',self.OnNewMail)
		comms.run('localhost',9000,'pymoos')


class FogRemove(AppCastingMOOSApp):

	def __init__(self):
		super(FogRemove, self).__init__()


	def RegisterVariables(self):
		comms.register('CAMERA_SIDE',0)
		comms.register('CAMERA_SIDE_CLEAN',0)
		comms.register('CAMERA_BOTTOM',0)

		comms.add_message_route_to_active_queue('OnNewMail','CAMERA_SIDE_CLEAN')
		comms.add_message_route_to_active_queue('OnNewMail','CAMERA_SIDE')
		comms.add_message_route_to_active_queue('OnNewMail','CAMERA_BOTTOM')
		return True
	

	def OnNewMail(self, msg):
		if msg.is_binary():
			binaryData = bytearray(msg.binary_data())
			img = np.frombuffer(binaryData,dtype=np.uint8).reshape((288,360,3))
			if msg.key() == "CAMERA_SIDE":
				imgClean = image_clear(img.copy())
				comms.notify_binary('CAMERA_SIDE_CLEAN',bytes(imgClean.data),pymoos.time());
			cv2.imshow(msg.key(),img)
			cv2.waitKey(1)

		return True


def image_clear(img):
	img_dark= cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) #Convert the image from BGR to gray

	channels = cv2.split(img)

	#====== Get the maximum value of each channel and get the dark channel of each image======
	a_max_dst = [ float("-inf") ]*len(channels) # record the maximum value of each channel
	for idx in xrange(len(channels)):
		a_max_dst[idx] = channels[idx].max()

	img_dark = cv2.min(channels[0],cv2.min(channels[1],channels[2]))

	#=========================================================================================
	img_dark = cv2.GaussianBlur(img_dark,(25,25),0) # Gaussian filtering the dark channel

	img_t=(255.-0.95*img_dark)/255.
	img_t=cv2.max(img_t,0.5)

	#================= Calculate t(x) and get the clear image==================================
	for idx in xrange(len(channels)):
		# channels[idx] = cv2.max(cv2.add(cv2.subtract(channels[idx].astype(np.float32),int(a_max_dst[idx]))/img_t,int(a_max_dst[idx])),0.0)
		channels[idx] = cv2.max(cv2.add(cv2.subtract(channels[idx].astype(np.float32),int(a_max_dst[idx]))/img_t,int(a_max_dst[idx])),0.0)/int(a_max_dst[idx])*255
		channels[idx] = channels[idx].astype(np.uint8)

	return cv2.merge(channels)



if __name__=="__main__":

	fogRemove = FogRemove()
	
	while True:
		time.sleep(33)


